1. Field of the Invention
The present invention relates generally to a method of constructing an environmental map using sonar sensors, and, more particularly, to a method of constructing an environmental map using sonar sensors which is capable of accurately representing an environment using the data of inexpensive sonar sensors which usually is very erroneous.
2. Description of the Related Art
A grid map, a discretized field that converts an environment into a spatial lattice (see S. Thrun et al., Probabilistic Robotics, MIT Press, 2002), is extremely easy to recognize and understand, and can be used for localization (see D. Fox, W. Burgard and S. Thrun, “Markov Localization for Mobile Robots in Dynamic Environments”, Journal of Artificial Intelligence Research, Vol. 11, 1999, pp 391-427), path planning (see Howie Choset, Kevin M. Lynch, Seth Hutchinson, G. Kantor et al., Principle of Robot motion planning: Theory, Algorithms, and Implementations, MIT Press, 2005), collision avoidance (see R. Siegwart and I. R. Nourbakhsh, Introduction to Autonomous Mobile Robots, MIT Press, 2004), interaction between humans and robots (see W. Lee, H. Ryu, G. Yang, H. Kim, Y. Park, and S. Bang, “Design guidelines for map-based human-robot interfaces: A colocated workspace perspective”, International Journal of Industrial Ergonomics, vol. 37(5), 2007, pp 589-604), and multi-sensor fusion (see O. Cohen, Y. Edan, and E. Schechtman, “Statistical Evaluation Method for Comparing Grid Map Based Sensor Fusion Algorithms”, International Journal of Robotics Research, Vol. 25(2), 2006, pp 117-133). Various methods of range measurement have been used to build grid maps, including laser range finders (LRFs), cameras, and sonar sensors. Although LRFs are attractive in terms of accuracy, a laser beam penetrates glass objects (see D. Silver, D. Morales, L. Rekleitis, B. Lisien, and H. Choset, “Arc Carving: Obtaining Accurate, Low Latency Maps from Ultrasonic Range Sensors,” In Proceedings of the IEEE International Conference on Robotics and Automation, Vol. 2, 2004, pp 1554-1561). Cameras are sensitive to the illumination level (see M. Hebert, “Active and Passive Range Sensing for Robotics,” In Proceedings of the IEEE International Conference on Robotics and Automation, 2000, pp. 102-110). Sonar sensors are designed to measure the range to the closest obstacle in their beamwidth, and are not subject to problems of penetration or sensitivity to illumination. Furthermore, since sonar sensors are much less expensive than LRFs, they are attractive for practical indoor service robots such as a robotic vacuum cleaner.
Despite their advantages, sonar sensors suffer from two well known characteristic problems: incorrect measurements and angular uncertainty.
Incorrect Measurements: Sonar sensors frequently fail to detect the nearest obstacle because of undesirable reflections (see FIGS. 1A, 1B and 1C). A reflection causes an incorrect reading that can form ghost obstacles or omit real obstacles from the map. Experiments in general indoor environments have shown that about more than half of sonar measurements are spurious. This will be described in detail below.
Angular Uncertainty: Sonar sensors directly provide range information about the nearest obstacle, but not angular information (see H. Choset, K. Nagatani and N. Lazar, “The arc-transversal median algorithm: a geometric approach to increasing ultrasonic sensor azimuth accuracy”, IEEE Transactions on Robotics and Automation, Vol. 19 (3), 2003, pp 513-521). The uncertainty can hide narrow openings (see L. Kleeman and R. Kuc, “Sonar Sensing”, in Handbook of Robotics, edited by B. Siciliano and O. Khatib, Springer, 2008), and distort the map.
Under these problems, a new grid-mapping method called the Conflict Evaluated Maximum Approximated Likelihood (CEMAL) approach is proposed in the present invention. It starts with the maximum likelihood (ML) approach due to its effectiveness for managing angular uncertainty (see S. Thrun, “Learning Occupancy Grid Maps with Forward Sensor Models,” Autonomous Robots, Vol. 15, 2003, pp 111-127). Despite of this advantage, however, the ML approach has two drawbacks: erroneous maps (The map contains ghost obstacles, or fails to show real obstacles due to the effects of incorrect measurements) and a heavy computational load (The computational complexity of the ML approach is O(2kn) where k is the number of cells and n is the number of sonar readings).
It have been found that conflict cells, which will be presented in detail below, are related to these side effects as follows:
Erroneous Map: As conflict cells are caused only by incorrect measurements, it is essential to remove them. To do this, the conflict evaluation with sound pressure (CEsp) method, which distinguishes the incorrect readings that cause conflict cells, is proposed. The incorrect readings are filtered using the CEsp method, and then the erroneous parts can be reduced.
Heavy Computational load: When there are no conflict cells, the ML approach can be converted to a simple logic process that has light O(n) computational complexity by an approximation of the likelihood. This is the maximum approximated likelihood (MAL) approach.
The CEMAL approach thus consists of two layers: the filtering layer (CEsp method) and the fusion layer (MAL approach). When conflict cells occur, the CEsp method detects incorrect readings. Only the correct readings are fused into a grid map using the MAL approach. Our main contributions are as follows:
CEMAL inherits the angular uncertainty handling capability of the ML approach. Unlike the ML approach, however, CEMAL eliminates erroneous parts because the CEsp method filters them out. Therefore, the quality of a CEMAL grid map is excellent even using cheap sonar sensors. Based on two criteria given below, it is confirmed that the CEMAL grid map is about 92% accurate and that it can represent about 96% of an environment.
CEMAL has a light O(n) computational load that is comparable to the previous binary or trinary estimation approaches (see M. Ribo and A. Pinz, “A comparison of three uncertainty calculi for building sonar-based occupancy grids”, Robotics and Autonomous Systems, Vol. 35 (3-4), 2001, pp 201-209), and is very low compared to the O(2kn) load of the ML approach. In addition, it can work incrementally. Several experiments showed that CEMAL requires approximately 1 ms to process one measurement.
CEMAL does not require adjustment of parameters. Binary or trinary estimation approaches must regulate their own updating parameters carefully to establish an accurate map. CEMAL requires partial modification only when using a different kind of sonar sensor.
CEMAL works well even with only two sonar sensors. The possibility of using a low number of sonar sensors allows CEMAL to be used in commercial applications.
Related researches will be described below.
Grid mapping will now be described first.
Previous grid-mapping approaches can be classified into two categories: binary (or trinary) estimation and high-dimensional optimization.
Binary (or Trinary) Estimation
As finding a solution among the 2k possible maps is an intractable problem, binary or trinary estimation approaches decompose the high-dimensional problem into a collection of binary or trinary state estimation problems based on the assumption that each cell is independent of the others (see S. Thrun et al., Probabilistic Robotics, MIT Press, 2002). The posterior approach (PT) (see H. P. Moravec, “Sensor fusion in certainty grids for mobile robots”, AI Magazine, Vol. 9, 1988, pp. 61-74) calculates a posterior probability that measures the occupancy of the cell. Other approaches (see J. Borenstein and Y. Koren, “Histogramic in-motion mapping for mobile robot obstacle avoidance,” IEEE Transactions on Robotics and Automation, Vol. 7(4), 1991, pp 535-539) use a center-line model of the sonar sensor, and rely on the number of empty or occupied observations to estimate the state of each cell. The Dempster-Shafer approach (DS) (see R. R. Murphy, “Dempster-Shafer theory for sensor fusion in autonomous mobile robots”, IEEE Transactions on Robotics and Automation, Vol. 14 (2), 1998, pp 197-206) infers a mass function that indicates whether a cell is occupied, empty, or in an unknown state based on the Dempster-Shafer theory (see G. Shafer, “A Mathematical Theory of Evidence”, Princeton University Press, 1976). The fuzzy approach (FZ) (see G. Oriolo, G. Ulivi and M. Vendittelli, “Fuzzy maps: A new tool for mobile robot perception and planning”, Journal of Robotic Systems, Vol. 14 (3), 1997, pp 179-197), based on the theory of fuzzy sets (see L. A. Zadeh, “Outline of a new approach to the analysis of complex systems and decision process”, IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-3 (1), 1973, pp 28-44), quantifies the possibility that indicates a cell belongs to an obstacle, and then determines safe cells that are free from obstacles. As these approaches explicitly or implicitly assume independence, they reduce the enormous computational burden and the computational complexity to O(n). Because a measurement requires a constant time, the complexity is linear with the total number of measurements. However, the resulting grid map is defective in terms of representing narrow openings because the angular uncertainty is not handled appropriately by the assumption of the independence. Furthermore, these approaches require a tuning process whereby the updating parameters are regulated to acquire an accurate map.
High-Dimensional Optimization
Unlike the above approaches, the ML approach (see S. Thrun, “Learning Occupancy Grid Maps with Forward Sensor Models,” Autonomous Robots, Vol. 15, 2003, pp 111-127) uses a likelihood of sensor measurements without the assumption of the independence of other cells, and acquires a grid map that maximizes that likelihood. As the ML approach suffers from a heavy computational load, the expectation-maximization (EM) algorithm (see A. P. Dempster, N. M. Laird and D. B. Rubin, “Maximum Likelihood from Incomplete Data Via EM Algorithm,” Journal of the Royal Statistical Society Series B—Methodological, Vol. 39 (1), 1977, pp 1-38) is used in the paper “S. Thrun, “Learning Occupancy Grid Maps with Forward Sensor Models”, Autonomous Robots, Vol. 15, 2003, pp 111-127.” This solution reduces the computational complexity to O(nk) per iteration due to O(n) for E-step and O(nk) for M-step. The expectation of each measurement is calculated for the E-step while for the M-step, the state of each cell is reversed and the change in expectation of related measurements is examined. However, the number of iterations of the EM tends to depend linearly on the size of the search space 2k for the worst case. In addition, the EM algorithm may fall into a local minimum and cannot process data incrementally.
Coping with sonar sensor characteristics will now be described next.
Aside from mapping, previous research on methods for coping with the characteristics of sonar sensors can be divided into two groups. The first considered handling of the angular uncertainty of sonar sensors, and the second considered the detection of incorrect measurements.
Handling Angular Uncertainty
Arc maps (see D. Baskent and B. Barshan, “Surface profile determination from multiple sonar data using morphological processing,” International Journal of Robotics Research, Vol. 18 (8), 1999, pp 788-808) have been developed to deal with the angular uncertainty. The arc map shows an environment with only a collection of arcs, and obstacles can be located anywhere on the arcs. The arc transversal median (ATM) method (see H. Choset, K. Nagatani and N. Lazar, “The arc-transversal median algorithm: a geometric approach to increasing ultrasonic sensor azimuth accuracy”, IEEE Transactions on Robotics and Automation, Vol. 19 (3), 2003, pp 513-521) gathers intersections of sonar arcs, and extracts the median points from them. The arc-carving (AC) method (see D. Silver, D. Morales, L. Rekleitis, B. Lisien, and H. Choset, “Arc Carving: Obtaining Accurate, Low Latency Maps from Ultrasonic Range Sensors”, In Proceedings of the IEEE International Conference on Robotics and Automation, Vol. 2, 2004, pp 1554-1561) eliminates portions of sonar arcs that are contradicted by subsequent sonar readings, and derives the mean point of the remaining arc. The directional maximum (DM) method (see B. Barshan, “Directional Processing of Ultrasonic Arc Maps and its Comparison with Existing Techniques”, International Journal of Robotics Research, Vol. 26(8), 2007, pp 797-820) uses a direction of interest, and a cell that has a maximum intersection count along that direction is selected.
Detecting Incorrect Measurements
Several techniques have been developed for filtering out incorrect measurements, and these can be divided into four classes:
The first class eliminates incorrect sonar measurements by clustering. The random sample consensus/Gaussian filtering (RANSAC/GF) method (see A. Burguera, Y. Gonzalez and G. Oliver, “Sonar Scan Matching by Filtering Scans using Grids of Normal Distributions”, The International Conference on Intelligent Autonomous Systems, 2008, pp 64-73) establishes a Gaussian distribution with the RANSAC clustering (see M. A. Fischler and R. C. Bolles, “Random sample consensus: A paradigm for model fitting with application to image analysis and automated cartography”, Communications of the ACM, Vol. 24 (6), 1981, pp 381-395); readings that do not fit the distribution are rejected as outliers.
The second class discriminates sonar measurements that form geometric primitives, such as lines and points. The region of constant depth (RCD) matching method (see R. Kuc, and M. W. Siegel, “Physically based simulation model for acoustic sensor robot navigation”, IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 9 (6), 1987, pp 766-778) uses a geometric constraint based on the radius of a circle. Only sonar readings that satisfy this constraint are considered correct. The RCD matching method is useful for finding corners, planes, and cylinders. The feature prediction (FP) method (see S. O'Sullivan, J. J. Collins, M. Mansfield, D. Haskett, M. Eaton, “Linear feature prediction for confidence estimation of sonar readings in map building”, In Proceedings of the International Symposium on Artificial Life and Robotics (AROB), Japan, 2004) assigns a confidence measure to each sonar reading to indicate whether it is reliable. The position and orientation of features established by hypothetical obstacles in the local space of the robot determine the reliability.
The third class limits the maximum admissible range adaptively. The bounding box method (see E. Ivanjko, I. Petrovic and K. Macek, “Improvements of occupancy grid maps by sonar data corrections”, In Proceedings of FIRA Robot Soccer World Congress, Vienna, Austria, 2003) was introduced to correct unreliable sonar sensor readings, creating a bounding box from four (front, back, left, and right)-directional sensor readings. If a sensor reading falls outside the box, its range is modified to be on the border of the box. In the navigable Voronoi diagram (NVD) method (see K. Lee and W. K. Chung, “Navigable voronoi diagram: a local path planner for mobile robots using sonar sensors”, In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, 2007, pp 2813-2818), the diagram is generated by instantaneous measurements, and sonar readings beyond the diagram are excluded.
The fourth class focuses on the consistency of sonar information. The sonar probabilistic analysis of conflicts (spAC) method (see A. Burguera, Y. Gonzalez and G. Oliver, “Probabilistic sonar filtering in scan matching localization”, In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, 2007, pp 4158-4163) iteratively determines the probability of each sonar reading based on the occurrence of conflict cells. The CEsp method is included in this class, and will be described below. The conflict evaluation method based on a logical approach was proposed (see K. Lee, I. H. Suh, S. Oh, and W. K. Chung, “Conflict Evaluation Method for Grid Maps using Sonar Sensors,” In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008, pp 2908-2914) in precursor research into the CEsp method. However, because the previous approach has exceptional cases, it is difficult to apply generally.